#include "DXUT.h"
#include "..\\Utility\\UtilDefs.h"
#include "XYZFileIO.h"
#include "..\\Utility\\D3D\\D3DUtilityFunction.h"
#include "..\\Utility\\Math\\SVD.h"

const float CXYZFileIO::MINVAL = -999999.0f;

CXYZFileIO::CXYZFileIO(void)
{
	//File handle
	fp = NULL;
	/***************************************************************************************/
	/*member variables for .xyz file*/	
	/***************************************************************************************/
	/*line 1*/
	//string constant 
	m_strConstL1 = NULL;
	/*line 2*/
	m_iSoftwareType = 0;
	m_iMajorVers = 0; m_iMinorVers = 0;	m_iBugVers = 0;
	m_strSoftwareDate = NULL;
	/*line 3*/
	m_iIntensOriginX = 0; m_iIntensOriginY = 0;
	m_iIntensWidth = 0; m_iIntensHeight = 0;
	m_iNBuckets = 0;
	m_uiIntensRange = 0;
	/*line 4*/
	m_iPhaseOriginX = 0; m_iPhaseOriginY = 0;
	m_iPhaseWidth = 0; m_iPhaseHeight = 0;
	/*line 5*/
	m_strComment = NULL;
	/*line 6*/
	m_strPartSerNum = NULL;
	/*line 7*/
	m_strPartNum = NULL;
	/*line 8*/
	m_iSource = 0;
	m_fIntfScaleFactor = 0.0f;
	m_fWavelengthIn = 0.0f;
	m_fNumericAperture = 0.0f;
	m_fObliquityFactor = 0.0f;
	m_fMagnification = 0.0f;
	m_fCameraRes = 0.0f;
	m_iTimeStamp = 0;
	/*line 9*/
	m_iCameraWidth = 0; m_iCameraHeight = 0;
	m_iSystemType = 0;
	m_iSystemBoard = 0;
	m_iSystemSerial = 0;
	m_iInstrumentId = 0;
	m_strObjectiveName = NULL;
	/*line 10*/
	m_iAcquireMode = 0;
	m_iIntensAvgs = 0;
	m_iPZTCal = 0;
	m_iPZTGain = 0;
	m_iPZTGainTolerance = 0;
	m_iAGC = 0;
	m_fTargetRange = 0.0f;
	m_fLightLevel = 0.0f;
	m_iMinMod = 0;
	m_iMinModPts = 0;
	/*line 11*/
	m_iPhaseRes = 0;
	m_iPhaseAvgs = 0;
	m_iMinimumAreaSize = 0;
	m_iDisconAction = 0;
	m_fDisconFilter = 0;
	m_iConnectionOrder = 0;
	m_iRemoveTiltBias = 0;
	m_iDataSign = 0;
	m_iCodeVType = 0;
	/*line 12*/
	m_iSubtractSysErr = 0;
	m_strSysErrFile = NULL;
	/*line 13*/
	m_fRefractiveIndex = 0.0f;
	m_fPartThickness = 0.0f;
	/*line 14*/
	m_strZoomDesc = NULL;
	/***************************************************************************************/
	//height map data
	m_arrHeightMapData = NULL;
	m_iHMDataWidth = -1;
	m_iHMDataHeight = -1;
	m_fHeightRange = 0.0f;

	m_bDataReady = false;
	m_arrVertices.clear();

	//width, height
	m_fSampleWidth = 140;
	m_fSampleHeight = 110;

	//string filename
	ZeroMemory(m_strXYZFileName,sizeof(char)*MAX_PATH);
}

CXYZFileIO::~CXYZFileIO(void)
{
	reset();
}

void CXYZFileIO::reset(void)
{
	SAFE_CLOSEFILE(fp);
	SAFE_DELETE_ARRAY(m_strConstL1);
	SAFE_DELETE_ARRAY(m_strSoftwareDate);
	SAFE_DELETE_ARRAY(m_strComment);
	SAFE_DELETE_ARRAY(m_strPartSerNum);
	SAFE_DELETE_ARRAY(m_strPartNum);
	SAFE_DELETE_ARRAY(m_strObjectiveName);
	SAFE_DELETE_ARRAY(m_strSysErrFile);
	SAFE_DELETE_ARRAY(m_strZoomDesc);
	SAFE_DELETE_ARRAY(m_arrHeightMapData);
	m_arrVertices.clear();
	m_arrNormalAreaWeighted.clear();
	m_arrNoDataPtIdx.clear();
	ZeroMemory(m_strXYZFileName,sizeof(char)*MAX_PATH);
}

int CXYZFileIO::LoadFromFile(const char* strFileName,float fSampleWidth, float fSampleHeight)
{
	if(strFileName == NULL)
		return -1;

	//reset current object
	reset();
	if( (fp = fopen( strFileName, "r" )) == NULL )
		 return -1;
	//start loading file
	printf("start loading %s file\n",strFileName);
	char* tmpArray = NULL;
	NEW_CHAR_ARRAY(tmpArray, MAX_PATH);
	/*line 1*/
	printf("File header:\n");
	//string constant 
	NEW_CHAR_ARRAY(m_strConstL1,MAX_PATH);
	fgets(tmpArray,MAX_PATH,fp);
	strcpy(m_strConstL1,tmpArray);
	printf("%s",m_strConstL1);
	/*line 2*/
	NEW_CHAR_ARRAY(m_strSoftwareDate,MAX_PATH);
	ZeroMemory(tmpArray, MAX_PATH);
	fgets(tmpArray, MAX_PATH, fp);
	char* tmpStrStart = strchr(tmpArray, '"');
	strcpy(m_strSoftwareDate, tmpStrStart);
	*tmpStrStart = '\0';
	sscanf(tmpArray,"%d %d %d %d",&m_iSoftwareType,&m_iMajorVers,&m_iMinorVers,&m_iBugVers);
	printf("%d %d %d %d %s",m_iSoftwareType,m_iMajorVers,m_iMinorVers,m_iBugVers,m_strSoftwareDate);
	/*line 3*/
	fscanf(fp,"%d %d %d %d %d %u\n",&m_iIntensOriginX, &m_iIntensOriginY, &m_iIntensWidth, &m_iIntensHeight, &m_iNBuckets, &m_uiIntensRange);
	printf("%d %d %d %d %d %u\n",m_iIntensOriginX, m_iIntensOriginY, m_iIntensWidth, m_iIntensHeight, m_iNBuckets, m_uiIntensRange);
	/*line 4*/
	fscanf(fp,"%d %d %d %d\n",&m_iPhaseOriginX,&m_iPhaseOriginY,&m_iPhaseWidth,&m_iPhaseHeight);
	printf("%d %d %d %d\n",m_iPhaseOriginX,m_iPhaseOriginY,m_iPhaseWidth,m_iPhaseHeight);
	/*line 5*/
	NEW_CHAR_ARRAY(m_strComment,MAX_PATH);
	ZeroMemory(tmpArray,MAX_PATH);
	fgets(tmpArray, MAX_PATH, fp);
	strcpy(m_strComment,tmpArray);
	printf("%s",m_strComment);
	/*line 6*/
	NEW_CHAR_ARRAY(m_strPartSerNum, MAX_PATH);
	ZeroMemory(tmpArray, MAX_PATH);
	fgets(tmpArray, MAX_PATH, fp);
	strcpy(m_strPartSerNum, tmpArray);
	printf("%s",m_strPartSerNum);
	/*line 7*/
	NEW_CHAR_ARRAY(m_strPartNum, MAX_PATH);
	ZeroMemory(tmpArray, MAX_PATH);
	fgets(tmpArray, MAX_PATH, fp);
	strcpy(m_strPartNum, tmpArray);
	printf("%s",m_strPartNum);
	/*line 8*/
	fscanf(fp,"%d %f %f %f %f %f %f %d\n",&m_iSource, &m_fIntfScaleFactor, &m_fWavelengthIn, &m_fNumericAperture, &m_fObliquityFactor, &m_fMagnification, &m_fCameraRes, &m_iTimeStamp);
	printf("%d %f %f %f %f %f %f %d\n",m_iSource, m_fIntfScaleFactor, m_fWavelengthIn, m_fNumericAperture, m_fObliquityFactor, m_fMagnification, m_fCameraRes, m_iTimeStamp);
	/*line 9*/
	NEW_CHAR_ARRAY(m_strObjectiveName, MAX_PATH);
	ZeroMemory(tmpArray, MAX_PATH);
	fgets(tmpArray, MAX_PATH, fp);
	tmpStrStart = strchr(tmpArray,'"');
	strcpy(m_strObjectiveName, tmpStrStart);
	*tmpStrStart = '\0';
	sscanf(tmpArray,"%d %d %d %d %d %d",&m_iCameraWidth,&m_iCameraHeight,&m_iSystemType,&m_iSystemBoard,&m_iSystemSerial,&m_iInstrumentId);
	printf("%d %d %d %d %d %d %s",m_iCameraWidth,m_iCameraHeight,m_iSystemType,m_iSystemBoard,m_iSystemSerial,m_iInstrumentId,m_strObjectiveName);
	/*line 10*/
	fscanf(fp,"%d %d %d %d %d %d %f %f %d %d\n",&m_iAcquireMode,&m_iIntensAvgs,&m_iPZTCal,&m_iPZTGain,&m_iPZTGainTolerance,&m_iAGC,&m_fTargetRange,&m_fLightLevel,&m_iMinMod,&m_iMinModPts);
	printf("%d %d %d %d %d %d %f %f %d %d\n",m_iAcquireMode,m_iIntensAvgs,m_iPZTCal,m_iPZTGain,m_iPZTGainTolerance,m_iAGC,m_fTargetRange,m_fLightLevel,m_iMinMod,m_iMinModPts);
	/*line 11*/
	fscanf(fp,"%d %d %d %d %d %d %d %d %d\n",&m_iPhaseRes,&m_iPhaseAvgs,&m_iMinimumAreaSize,&m_iDisconAction,&m_fDisconFilter,&m_iConnectionOrder,&m_iRemoveTiltBias,&m_iDataSign,&m_iCodeVType);
	printf("%d %d %d %d %d %d %d %d %d\n",m_iPhaseRes,m_iPhaseAvgs,m_iMinimumAreaSize,m_iDisconAction,m_fDisconFilter,m_iConnectionOrder,m_iRemoveTiltBias,m_iDataSign,m_iCodeVType);
	/*line 12*/
	NEW_CHAR_ARRAY(m_strSysErrFile, MAX_PATH);
	ZeroMemory(tmpArray, MAX_PATH);
	fgets(tmpArray, MAX_PATH, fp);
	tmpStrStart = strchr(tmpArray,'"');
	strcpy(m_strSysErrFile, tmpStrStart);
	*tmpStrStart = '\0';
	sscanf(tmpArray, "%d", &m_iSubtractSysErr);
	printf("%d %s", m_iSubtractSysErr,m_strSysErrFile);
	/*line 13*/
	fscanf(fp, "%f %f\n",&m_fRefractiveIndex, &m_fPartThickness);
	printf("%f %f\n",m_fRefractiveIndex, m_fPartThickness);
	/*line 14*/
	NEW_CHAR_ARRAY(m_strZoomDesc, MAX_PATH);
	ZeroMemory(tmpArray, MAX_PATH);
	fgets(tmpArray, MAX_PATH, fp);
	strcpy(m_strZoomDesc,tmpArray);
	printf("%s",m_strZoomDesc);
	printf("End of header.\n");
	
	/***************************************************************************************/
	//height map data
	if(m_iPhaseWidth>0&&m_iPhaseHeight>0)
	{
		m_iHMDataWidth = m_iPhaseWidth;
		m_iHMDataHeight = m_iPhaseHeight;

		printf("Loading data part...\n");
		SAFE_DELETE_ARRAY(m_arrHeightMapData);
		m_arrHeightMapData = new float[m_iHMDataHeight*m_iHMDataWidth];
		int iXpos = 0, iYpos = 0;
		float fMinHeight = -MINVAL;
		float fMaxHeight = MINVAL;
		ZeroMemory(tmpArray, MAX_PATH);
		m_arrNoDataPtIdx.clear();
		for(int j = 0; j < m_iHMDataHeight; j++)
			for(int i = 0; i < m_iHMDataWidth; i++)
			{
				float* pHeight = (m_arrHeightMapData+j*m_iHMDataWidth+i);
				
				fgets(tmpArray,MAX_PATH,fp);
				char* tmp = strchr(tmpArray,'N');
				if(tmp==NULL)
					{
						sscanf(tmpArray, "%d %d %f", &iYpos, &iXpos, pHeight);
						if(*pHeight < fMinHeight)
							fMinHeight = *pHeight;
						if(*pHeight > fMaxHeight)
							fMaxHeight = *pHeight;
				}
				else
					{
						*pHeight = MINVAL;
						m_arrNoDataPtIdx.push_back(UnsolvedPoint(i,j));						
					}
				//printf("%d %d %f\n", iYpos, iXpos, *(m_arrHeightMapData+j*m_iPhaseWidth+i));
			}
			//convert the datarange to [0,1]
			m_fHeightRange = fMaxHeight - fMinHeight;
			for(int j = 0; j < m_iHMDataHeight; j++)
				for(int i = 0; i < m_iHMDataWidth; i++)
				{
					float* pHeight = (m_arrHeightMapData+j*m_iHMDataWidth+i);
					if(!(*pHeight < MINVAL + 1.0))
						*pHeight = (*pHeight - fMinHeight);///(fMaxHeight - fMinHeight);						
					else
						//for "No data" point, height = 0
						*pHeight = -0.01f;
				}
		
		m_bDataReady = true;
		printf("Loading data finished.\n");
		printf("Number of \'No data\' points is %d, ratio:%f%%\n",m_arrNoDataPtIdx.size(), GetNoDataRatio()*100);
		printf("Height range is Min:%f, Max:%f, Diff:%f.\n", fMinHeight, fMaxHeight, m_fHeightRange);
	}
	else
		printf("Error: No data in File...");
	
	SAFE_DELETE_ARRAY(tmpArray);
	SAFE_CLOSEFILE(fp);

	m_fSampleWidth = fSampleWidth;
	m_fSampleHeight = fSampleHeight;
	//set file name
	SetStrXYZFilePathName(strFileName);
	return 0;
}

/*
Finding the orthogonal distance regression plane is an eigenvector 
problem. It is quite involved, as you suspected. The best solution 
utilizes the Singular Value Decomposition (SVD). I hope you have 
access to a good linear algebra software package.

Starting with the distance from a point to a plane, we wish to find 
a, b, c and d such that we minimize

f(a,b,c,d) = Sum [|axi + byi + czi + d|^2 / (a^2 + b^2 + c^2)]

If we set the partial derivative with respect to d equal to zero, we 
can solve for d to get

d = -(a*x0 + b*y0 + c*z0)

where (x0, y0, z0) is the centroid of the data. This means that the 
least squares plane contains the centroid. If we substitute it back 
into the equation for the plane we get

a(x - x0) + b(y - y0) + c(z - z0) = 0

We can rewrite f(a,b,c,d) like this

f(a,b,c) = Sum [|a(xi-x0) + b(yi-y0) + c(zi-z0)|^2 / (a^2+b^2+c^2)]

Now we are going to switch over to a matrix representation. I will 
be using both the upper and lower cases of some letters. I hope that 
does not cause confusion.

Let's define v and M such that

 T
v  = [a  b  c]

--                             --
| x1 - x0    y1 - y0    z1 - z0 |
| x2 - x0    y2 - y0    z2 - z0 |
M =  |    .          .          .    |
|    .          .          .    |
|    .          .          .    |
| xn - x0    yn - y0    zn - z0 |
--                             --

If you multiply the matrices out, you will see that f(a,b,c) becomes

         T T          T
f(v) = (v M )(Mv) / (v v)

   T  T        T
= v (M M)v / (v v)

                    T
Let's define A = M M, which when divided by the number of data points 
becomes the covariance matrix of the data.

f(v) is called a Rayleigh Quotient. It is minimized by the eigenvector 
of A that corresponds to its smallest eigenvalue.

We could compute the eigenvectors of A, but this is not needed. The 
SVD of M is
       T
M = USV 

where S is a diagonal matrix containing the singular values of M, the 
columns of V are its singular vectors, and U is an orthogonal matrix.

                    T
Now            A = M M

      T T    T
= (USV ) (USV )

      T T      T
= (V S U ) (USV )

   2 T 
= V S V

This decomposition of A diagonalizes the matrix and provides an 
eigenvector decomposition. It means that the eigenvalues of A are the 
squares of the singular values of M, and the eigenvectors of A are the 
singular vectors of M.

To conclude, the orthogonal least squares 3D plane contains the 
centroid of the data, and its normal vector is the singular vector of 
M corresponding to its smallest singular value.
*/
bool CXYZFileIO::GenerateVerticesPos(float fStartOffsetX, float fStartOffsetY)
{
	if(!m_bDataReady)
		return false;

	//create vertex buffer
	float fStepX = m_fSampleWidth/(m_iHMDataWidth-1);
	float fStepY = m_fSampleHeight/(m_iHMDataHeight-1);
	float fStartX = fStartOffsetX*m_fSampleWidth;
	float fStartY = fStartOffsetY*m_fSampleHeight;
	for(int j = 0; j < m_iHMDataHeight; j++)
		for(int i = 0; i < m_iHMDataWidth; i++)
		{	
			VertexNorm tmpPos;
			int idx = j*m_iHMDataWidth + i;
			CD3DUtilityFunction::FillVec3(i*fStepX+fStartX, j*fStepY+fStartY, m_arrHeightMapData[idx], &(tmpPos.Position));			
			m_arrVertices.push_back(tmpPos);
		}
	return true;
}

/*
You might want to check out principal component analysis. You can calculate a 3x3 covariance matrix for 
your set of points and then determine the eigenvectors corresponding to the two largest eigenvalues. 
These two vectors are parallel to the best-fit plane, so just cross them to get the plane's normal, 
and then adjust the plane's fourth coordinate so that it passed through the average position of your point set.

Here I am using 3x3 covariance matrix to save the computation for svdcmp();

Note! the array element index value for Numerical Recipes functions starts from 1 not 0!
*/
bool CXYZFileIO::EstimateNormalForEachPoint(unsigned int iNeighborSize)
{
	if(!m_bDataReady || iNeighborSize < 2)
		return false;
	//Firstly fill the vertices array using positions
	m_arrVertices.clear();
	GenerateVerticesPos(-0.5f,-0.5f);

	//start to estimate normal
	std::vector<UnsolvedPoint> NoDataPts_Arr;
	printf("Start to compute normal for each point!\n");
	unsigned int iNeg = 0;
	//allocate memory space
	double** CovMat = new double*[4];
	double **V = new double*[4];
	double W[4];
	for(int i = 0; i < 4; i++)
	{
		CovMat[i] = new double[4];
		V[i] = new double[4];
	}
	//Loop each point
	for (unsigned int iY = 0; iY < m_iHMDataHeight; iY++)
		for (unsigned int iX = 0; iX < m_iHMDataWidth; iX++)
		{
			unsigned int iCurPtIdx = iY*m_iHMDataWidth + iX;
			if(iCurPtIdx%(m_iHMDataWidth*10)==0)
				printf("*");
			//'No-data' point case
			if(m_arrHeightMapData[iCurPtIdx] < 0)
			{
				NoDataPts_Arr.push_back(UnsolvedPoint(iX,iY));
				continue;
			}
			//1.compute the average position
			int iStep = iNeighborSize/2;
			D3DXVECTOR3 iCenterPos(0,0,0);
			std::vector<D3DXVECTOR3> LocalPts_Arr;
			for (int j = -1; j <= iStep; j++)
				for(int i = -1; i <= iStep; i++)
				{
					int idx_x = i+iX;
					int idx_y = j+iY;
					if(idx_x < 0 || idx_y < 0 || idx_x >= m_iHMDataWidth || idx_y >= m_iHMDataHeight)
						continue;

					unsigned int idx = idx_y*m_iHMDataWidth+idx_x;
					//'No-data' point case
					if(m_arrHeightMapData[idx] < 0)
						continue;

					iCenterPos += m_arrVertices[idx].Position;					
					LocalPts_Arr.push_back(m_arrVertices[idx].Position);
				}
				//there is no enough valid points in neighborhood!
				unsigned int iNumLocalPts = LocalPts_Arr.size();
				if (iNumLocalPts < 3)
				{
					NoDataPts_Arr.push_back(UnsolvedPoint(iX,iY));
					continue;
				}

				//Average position
				iCenterPos /= iNumLocalPts;

				//2.Construct convariance matrix M*M_T
				std::vector<double> xvec;
				std::vector<double> yvec;
				std::vector<double> zvec;
				for(int i = 0; i < iNumLocalPts; i++)
				{
					xvec.push_back(LocalPts_Arr[i].x - iCenterPos.x);
					yvec.push_back(LocalPts_Arr[i].y - iCenterPos.y);
					zvec.push_back(LocalPts_Arr[i].z - iCenterPos.z);
				}
				//clear memory
				for(int i = 0; i < 4; i++)
				{
					ZeroMemory(CovMat[i],sizeof(double)*4);
					ZeroMemory(V[i],sizeof(double)*4);
				}
				ZeroMemory(W,sizeof(double)*4);

				//compute elements
				CovMat[1][1] = 0;
				for(int i =0; i < iNumLocalPts; i++)
					CovMat[1][1] += xvec[i] * xvec[i];

				CovMat[1][2] = 0;
				for(int i =0; i < iNumLocalPts; i++)
					CovMat[1][2] += xvec[i] * yvec[i];

				CovMat[1][3] = 0;
				for(int i =0; i < iNumLocalPts; i++)
					CovMat[1][3] += xvec[i] * zvec[i];

				CovMat[2][1] = CovMat[1][2];
				CovMat[2][2] = 0;
				for(int i =0; i < iNumLocalPts; i++)
					CovMat[2][2] += yvec[i] * yvec[i];

				CovMat[2][3] = 0;
				for(int i =0; i < iNumLocalPts; i++)
					CovMat[2][3] += yvec[i] * zvec[i];

				CovMat[3][1] = CovMat[1][3];
				CovMat[3][2] = CovMat[2][3];
				CovMat[3][3] = 0;
				for(int i =0; i < iNumLocalPts; i++)
					CovMat[3][3] += zvec[i] * zvec[i];

				if (!SVD::svdcmp(CovMat,3,3,W,V))
				{
#ifdef _DEBUG
					OUTPUTERRORINFO;
#endif
					return false;
				}

				int iMinIdx = -1;
				int iMaxIdx = -1;
				double minVal = DBL_MAX;
				double maxVal = DBL_MIN;
				for(int k = 1; k <4; k++)
				{
					if(W[k] < minVal)
					{
						minVal = W[k];
						iMinIdx = k;
					}

					if(W[k] > maxVal)
					{
						maxVal = W[k];
						iMaxIdx = k;
					}
				}
				
				/*
				//int idx1 = -1;
				//for(int k = 1; k <4; k++)
				//{
				//	if(k!=iMinIdx)
				//	{idx1=k; break;}
				//}
				//int idx2 = -1;
				//for(int k = 1; k <4; k++)
				//{
				//	if(k!=iMinIdx&&k!=idx1)
				//	{idx2=k; break;}
				//}
				//int iMidIdx = -1;
				//for(int k = 1; k <4; k++)
				//{
				//	if(k!=iMinIdx && k!=iMaxIdx)
				//		iMidIdx = k;
				//}

				D3DXVECTOR3 PlaneVecs1;
				PlaneVecs1.x = V[iMaxIdx][1];
				PlaneVecs1.y = V[iMaxIdx][2];
				PlaneVecs1.z = V[iMaxIdx][3];

				D3DXVECTOR3 PlaneVecs2;
				PlaneVecs2.x = V[iMidIdx][1];
				PlaneVecs2.y = V[iMidIdx][2];
				PlaneVecs2.z = V[iMidIdx][3];

				D3DXVECTOR3 norm;
				//cross direction --> maxIdx x midIdx
				D3DXVec3Cross(&norm,&PlaneVecs1,&PlaneVecs2);
				*/
				D3DXVECTOR3 norm = D3DXVECTOR3(V[iMinIdx][1],V[iMinIdx][2],V[iMinIdx][3]);
				if(norm.z < 0)
				{
					//printf("(%f,%f,%f)\t", norm.x,norm.y,norm.z);
					norm *= -1.0f;
					//iNeg++;
				}
				//3. Fill the normal of current pt
				D3DXVec3Normalize(&(m_arrVertices[iCurPtIdx].Normal),&norm);
											

		}	
		printf("iNeg:%d\n",iNeg);
		//release memory
		for (int i = 0; i < 4; i++)
		{
			SAFE_DELETE_ARRAY(CovMat[i]);
			SAFE_DELETE_ARRAY(V[i]);
		}
		SAFE_DELETE_ARRAY(CovMat);
		SAFE_DELETE_ARRAY(V);
		
		//process normal of 'No-data' Point
		//use local triangle's normal vector
		for (unsigned int i = 0; i < NoDataPts_Arr.size();i++)
		{
			unsigned int idY = NoDataPts_Arr[i].iPtY;
			unsigned int idX = NoDataPts_Arr[i].iPtX;

			unsigned int iCurPtIdx = idY*m_iHMDataWidth + idX;
			unsigned int v2 = iCurPtIdx + 1;
			unsigned int v3 = iCurPtIdx + m_iHMDataWidth;
			unsigned int v4 = v2 + m_iHMDataWidth;
			
			int iCase = -1;
			//normal case (1)(v2-v1)x(v4-v1) (2)(v4-v1)x(v3-v1)
			if(idX < (m_iHMDataWidth-1) && idY < (m_iHMDataHeight-1))
			{
				D3DXVECTOR3 vec1 = m_arrVertices[v2].Position - m_arrVertices[iCurPtIdx].Position;//v2-v1
				D3DXVECTOR3 vec2 = m_arrVertices[v4].Position - m_arrVertices[iCurPtIdx].Position;//v4-v1

				D3DXVECTOR3 norm;
				D3DXVec3Cross(&norm,&vec1,&vec2);
				D3DXVECTOR3 norm1;
				D3DXVec3Normalize(&norm1,&norm);

				vec1 = m_arrVertices[v3].Position - m_arrVertices[iCurPtIdx].Position;//v3-v1
				D3DXVec3Cross(&norm,&vec2,&vec1);
				D3DXVECTOR3 norm2;
				D3DXVec3Normalize(&norm2,&norm);
				//simple average, and we can go to complex area-based way!!!
				norm = (norm1+norm2)/2.0f;
				D3DXVec3Normalize(&(m_arrVertices[iCurPtIdx].Normal),&norm);				
				iCase = 1;
				if(m_arrVertices[iCurPtIdx].Normal.z < 0)
				{
					printf("%d:(%f,%f,%f)\t", iCase, m_arrVertices[iCurPtIdx].Normal.x,m_arrVertices[iCurPtIdx].Normal.y,m_arrVertices[iCurPtIdx].Normal.z);
					iNeg++;
				}			
			}
			//(2)(v3-v1)x(v4-v1) (2)(v4-v1)x(v2-v1)
			else if (idY < (m_iHMDataHeight-1))
			{
				v2 = iCurPtIdx - 1;
				v3 = iCurPtIdx + m_iHMDataWidth;
				v4 = v2 + m_iHMDataWidth;

				D3DXVECTOR3 vec1 = m_arrVertices[v3].Position - m_arrVertices[iCurPtIdx].Position;//v3-v1
				D3DXVECTOR3 vec2 = m_arrVertices[v4].Position - m_arrVertices[iCurPtIdx].Position;//v4-v1

				D3DXVECTOR3 norm;
				D3DXVec3Cross(&norm,&vec1,&vec2);
				D3DXVECTOR3 norm1;
				D3DXVec3Normalize(&norm1,&norm);

				vec1 = m_arrVertices[v2].Position - m_arrVertices[iCurPtIdx].Position;//v2-v1
				D3DXVec3Cross(&norm,&vec2,&vec1);
				D3DXVECTOR3 norm2;
				D3DXVec3Normalize(&norm2,&norm);

				norm = (norm1+norm2)/2.0f;
				D3DXVec3Normalize(&(m_arrVertices[iCurPtIdx].Normal),&norm);	
				iCase = 2;
				if(m_arrVertices[iCurPtIdx].Normal.z < 0)
				{
					printf("%d:(%f,%f,%f)\t", iCase, m_arrVertices[iCurPtIdx].Normal.x,m_arrVertices[iCurPtIdx].Normal.y,m_arrVertices[iCurPtIdx].Normal.z);
					iNeg++;
				}			
			}
			//(3)(v3-v1)x(v4-v1) (2)(v4-v1)x(v2-v1)
			else if (idX < (m_iHMDataWidth-1))
			{
				unsigned int v2 = iCurPtIdx + 1;
				unsigned int v3 = iCurPtIdx - m_iHMDataWidth;
				unsigned int v4 = v2 - m_iHMDataWidth;
				
				D3DXVECTOR3 vec1 = m_arrVertices[v3].Position - m_arrVertices[iCurPtIdx].Position;//v3-v1
				D3DXVECTOR3 vec2 = m_arrVertices[v4].Position - m_arrVertices[iCurPtIdx].Position;//v4-v1

				D3DXVECTOR3 norm;
				D3DXVec3Cross(&norm,&vec1,&vec2);
				D3DXVECTOR3 norm1;
				D3DXVec3Normalize(&norm1,&norm);

				vec1 = m_arrVertices[v2].Position - m_arrVertices[iCurPtIdx].Position;//v2-v1
				D3DXVec3Cross(&norm,&vec2,&vec1);
				D3DXVECTOR3 norm2;
				D3DXVec3Normalize(&norm2,&norm);

				norm = (norm1+norm2)/2.0f;
				D3DXVec3Normalize(&(m_arrVertices[iCurPtIdx].Normal),&norm);
				iCase = 3;
				if(m_arrVertices[iCurPtIdx].Normal.z < 0)
				{
					printf("%d:(%f,%f,%f)\t", iCase, m_arrVertices[iCurPtIdx].Normal.x,m_arrVertices[iCurPtIdx].Normal.y,m_arrVertices[iCurPtIdx].Normal.z);
					iNeg++;
				}			
			}
			//(4)(v2-v1)x(v4-v1) (2)(v4-v1)x(v3-v1)
			else if (idX == (m_iHMDataWidth-1) && idY == (m_iHMDataHeight-1))
			{
				unsigned int v2 = iCurPtIdx - 1;
				unsigned int v3 = iCurPtIdx - m_iHMDataWidth;
				unsigned int v4 = v2 - m_iHMDataWidth;

				D3DXVECTOR3 vec1 = m_arrVertices[v2].Position - m_arrVertices[iCurPtIdx].Position;//v2-v1
				D3DXVECTOR3 vec2 = m_arrVertices[v4].Position - m_arrVertices[iCurPtIdx].Position;//v4-v1

				D3DXVECTOR3 norm;
				D3DXVec3Cross(&norm,&vec1,&vec2);
				D3DXVECTOR3 norm1;
				D3DXVec3Normalize(&norm1,&norm);

				vec1 = m_arrVertices[v3].Position - m_arrVertices[iCurPtIdx].Position;//v3-v1
				D3DXVec3Cross(&norm,&vec2,&vec1);
				D3DXVECTOR3 norm2;
				D3DXVec3Normalize(&norm2,&norm);

				norm = (norm1+norm2)/2.0f;
				D3DXVec3Normalize(&(m_arrVertices[iCurPtIdx].Normal),&norm);	
				iCase = 4;
				if(m_arrVertices[iCurPtIdx].Normal.z < 0)
				{
					printf("%d:(%f,%f,%f)\t", iCase, m_arrVertices[iCurPtIdx].Normal.x,m_arrVertices[iCurPtIdx].Normal.y,m_arrVertices[iCurPtIdx].Normal.z);
					iNeg++;
				}			
			}

		}	
		printf("iNeg:%d\n",iNeg);
		printf("\n Normal computing is finished!\n");
	return true;
}

bool CXYZFileIO::EstimateNormalAreaWeighted_IgnoreNoDataPt()
{
	bool bRtVal = true;
	if(!m_bDataReady)
		return false;
	//Firstly fill the vertices array using positions
	if(m_arrVertices.size() != m_iHMDataWidth*m_iHMDataHeight)
	{
		m_arrVertices.clear();
		bRtVal = GenerateVerticesPos(-0.5f,-0.5f);
	}

	//find all the nodata point and its one-ring neighbors
	bool *bValidArr = new bool[m_iHMDataHeight*m_iHMDataWidth];
	memset(bValidArr,0,sizeof(bool)*m_iHMDataHeight*m_iHMDataWidth);//init to false
	for(unsigned int i = 0; i < m_arrNoDataPtIdx.size(); i++)
	{
		UnsolvedPoint PtIdx = m_arrNoDataPtIdx[i];
		unsigned int idx = PtIdx.iPtY*m_iHMDataWidth + PtIdx.iPtX;
		bValidArr[idx] = true;
		
		int iPtX1 = PtIdx.iPtX - 1;
		int iPtX2 = PtIdx.iPtX + 1;
		int iPtY1 = PtIdx.iPtY - 1;
		int iPtY2 = PtIdx.iPtY + 1;

		if(iPtX1 >= 0)
		{
			bValidArr[PtIdx.iPtY*m_iHMDataWidth + iPtX1] = true;
			if(iPtY1 >= 0)
				bValidArr[iPtY1*m_iHMDataWidth + iPtX1] = true;
			if(iPtY2 < m_iHMDataHeight)
				bValidArr[iPtY2*m_iHMDataWidth + iPtX1] = true;
		}

		if(iPtY1 >= 0)
		{
			bValidArr[iPtY1*m_iHMDataWidth + PtIdx.iPtX] = true;
			if(iPtX2 < m_iHMDataWidth)
				bValidArr[iPtY1*m_iHMDataWidth + iPtX2] = true;
		}

		if(iPtX2 < m_iHMDataWidth)
		{
			bValidArr[PtIdx.iPtY*m_iHMDataWidth + iPtX2] = true;
			if(iPtY2 < m_iHMDataHeight) 
				bValidArr[iPtY2*m_iHMDataWidth + iPtX2] = true;
		}

		if(iPtY2 < m_iHMDataHeight)
			bValidArr[iPtY2*m_iHMDataWidth + PtIdx.iPtX] = true;
	}
	
	//start to compute normal
	AreaWeightedVert* AreaWeightedVertArr = new AreaWeightedVert[m_iHMDataWidth*m_iHMDataHeight];
	//loop all the triangles
	printf("Area-based normal computation starts:!\n");
	for(int j = 0; j <(m_iHMDataHeight-1); j++)
		for(int i = 0; i <(m_iHMDataWidth-1); i++)
		{
			unsigned int idx1 = j*m_iHMDataWidth+i;
			unsigned int idx2 = idx1 + 1;
			unsigned int idx3 = idx1 + m_iHMDataWidth;
			unsigned int idx4 = idx2 + m_iHMDataWidth;
			bool bIdx1 = bValidArr[idx1];
			bool bIdx2 = bValidArr[idx2];
			bool bIdx3 = bValidArr[idx3];
			bool bIdx4 = bValidArr[idx4];
			if(bIdx1&&bIdx2&&bIdx3&&bIdx4)//==true --> ignore
				continue;
			if(idx1%(m_iHMDataWidth*10)==0) printf("*");

			D3DXVECTOR3 v1 = m_arrVertices[idx1].Position;
			D3DXVECTOR3 v2 = m_arrVertices[idx2].Position;
			D3DXVECTOR3 v3 = m_arrVertices[idx3].Position;
			D3DXVECTOR3 v4 = m_arrVertices[idx4].Position;
			//first triangle --> v1v2v4 -->(v2-v1)x(v4-v1)
			D3DXVECTOR3 normal1;
			D3DXVECTOR3 tmp;
			D3DXVECTOR3 v2v1 = v2-v1;
			D3DXVECTOR3 v4v1 = v4-v1;
			D3DXVec3Cross(&tmp,&v2v1,&v4v1);
			D3DXVec3Normalize(&normal1, &tmp);			
			float area1 = CD3DUtilityFunction::ComputeTriangleArea(v1,v2,v4);
			normal1*=area1;
			if(!bIdx1) {
				AreaWeightedVertArr[idx1].TotalNormal += normal1;
				AreaWeightedVertArr[idx1].TotalArea += area1;
			}
			if(!bIdx2) {
				AreaWeightedVertArr[idx2].TotalNormal += normal1;
				AreaWeightedVertArr[idx2].TotalArea += area1;
			}
			if(!bIdx4) {
				AreaWeightedVertArr[idx4].TotalNormal += normal1;
				AreaWeightedVertArr[idx4].TotalArea += area1;
			}
			//second triangle --> v1v4v3 --> (v4-v1)x(v3-v1)
			D3DXVECTOR3 normal2;
			D3DXVECTOR3 v3v1 = v3-v1;
			D3DXVec3Cross(&tmp,&v4v1,&v3v1);
			D3DXVec3Normalize(&normal2,&tmp);
			float area2 = CD3DUtilityFunction::ComputeTriangleArea(v1,v4,v3);
			normal2 *= area2;
			if(!bIdx1) {
				AreaWeightedVertArr[idx1].TotalNormal += normal2;
				AreaWeightedVertArr[idx1].TotalArea += area2;
			}
			if(!bIdx4) {
				AreaWeightedVertArr[idx4].TotalNormal += normal2;
				AreaWeightedVertArr[idx4].TotalArea += area2;
			}
			if(!bIdx3) {
				AreaWeightedVertArr[idx3].TotalNormal += normal2;
				AreaWeightedVertArr[idx3].TotalArea += area2;						
			}
		}

		m_arrNormalAreaWeighted.clear();
		for (unsigned int i = 0; i < m_arrVertices.size(); i++)
		{
			if(bValidArr[i])
				continue;
			D3DXVECTOR3 tmp = AreaWeightedVertArr[i].TotalNormal/AreaWeightedVertArr[i].TotalArea;
			D3DXVECTOR3 normal;
			D3DXVec3Normalize(&normal,&tmp);
#ifdef _DEBUG
			if(normal.z < 0)
				printf("neg");
#endif
			VertexNorm vert;
			vert.Position = m_arrVertices[i].Position;
			vert.Normal = normal;
			m_arrNormalAreaWeighted.push_back(vert);
		}

		//delete array
		SAFE_DELETE_ARRAY(AreaWeightedVertArr);
		SAFE_DELETE_ARRAY(bValidArr);
		printf("finished!\n");
		return bRtVal;
}

bool CXYZFileIO::EstimateNormalAreaWeighted()
{
	bool bRtVal = true;
	if(!m_bDataReady)
		return false;
	//Firstly fill the vertices array using positions
	if(m_arrVertices.size() != m_iHMDataWidth*m_iHMDataHeight)
	{
		m_arrVertices.clear();
		bRtVal = GenerateVerticesPos(-0.5f,-0.5f);
	}

	//start to compute normal
	AreaWeightedVert* AreaWeightedVertArr = new AreaWeightedVert[m_iHMDataWidth*m_iHMDataHeight];
	//loop all the triangles
	printf("Area-based normal computation starts:!\n");
	for(int j = 0; j <(m_iHMDataHeight-1); j++)
		for(int i = 0; i <(m_iHMDataWidth-1); i++)
		{
			unsigned int idx1 = j*m_iHMDataWidth+i;
			unsigned int idx2 = idx1 + 1;
			unsigned int idx3 = idx1 + m_iHMDataWidth;
			unsigned int idx4 = idx2 + m_iHMDataWidth;

			if(idx1%(m_iHMDataWidth*10)==0) printf("*");

			D3DXVECTOR3 v1 = m_arrVertices[idx1].Position;
			D3DXVECTOR3 v2 = m_arrVertices[idx2].Position;
			D3DXVECTOR3 v3 = m_arrVertices[idx3].Position;
			D3DXVECTOR3 v4 = m_arrVertices[idx4].Position;
			//first triangle --> v1v2v4 -->(v2-v1)x(v4-v1)
			D3DXVECTOR3 normal1;
			D3DXVECTOR3 tmp;
			D3DXVECTOR3 v2v1 = v2-v1;
			D3DXVECTOR3 v4v1 = v4-v1;
			D3DXVec3Cross(&tmp,&v2v1,&v4v1);
			D3DXVec3Normalize(&normal1, &tmp);			
			float area1 = CD3DUtilityFunction::ComputeTriangleArea(v1,v2,v4);
			normal1*=area1;
			AreaWeightedVertArr[idx1].TotalNormal += normal1;
			AreaWeightedVertArr[idx1].TotalArea += area1;
			AreaWeightedVertArr[idx2].TotalNormal += normal1;
			AreaWeightedVertArr[idx2].TotalArea += area1;
			AreaWeightedVertArr[idx4].TotalNormal += normal1;
			AreaWeightedVertArr[idx4].TotalArea += area1;
			
			//second triangle --> v1v4v3 --> (v4-v1)x(v3-v1)
			D3DXVECTOR3 normal2;
			D3DXVECTOR3 v3v1 = v3-v1;
			D3DXVec3Cross(&tmp,&v4v1,&v3v1);
			D3DXVec3Normalize(&normal2,&tmp);
			float area2 = CD3DUtilityFunction::ComputeTriangleArea(v1,v4,v3);
			normal2 *= area2;
			AreaWeightedVertArr[idx1].TotalNormal += normal2;
			AreaWeightedVertArr[idx1].TotalArea += area2;
			AreaWeightedVertArr[idx4].TotalNormal += normal2;
			AreaWeightedVertArr[idx4].TotalArea += area2;
			AreaWeightedVertArr[idx3].TotalNormal += normal2;
			AreaWeightedVertArr[idx3].TotalArea += area2;						
			
		}

		m_arrNormalAreaWeighted.clear();
		for (unsigned int i = 0; i < m_arrVertices.size(); i++)
		{
			D3DXVECTOR3 tmp = AreaWeightedVertArr[i].TotalNormal/AreaWeightedVertArr[i].TotalArea;
			D3DXVECTOR3 normal;
			D3DXVec3Normalize(&normal,&tmp);
#ifdef _DEBUG
			if(normal.z < 0)
				printf("neg");
#endif
			VertexNorm vert;
			vert.Position = m_arrVertices[i].Position;
			vert.Normal = normal;
			m_arrNormalAreaWeighted.push_back(vert);
		}

		//delete array
		SAFE_DELETE_ARRAY(AreaWeightedVertArr);
		printf("finished!\n");
		return bRtVal;
}

void CXYZFileIO::SetStrXYZFilePathName(const char* strFN)
{
	ZeroMemory(m_strXYZFileName,sizeof(char)*MAX_PATH);
	sprintf_s(m_strXYZFileName,strFN);
}